#include "FrameBase.h"
#include "math.h"

void setBit(canU8_t* Data, canU32_t data, const canU8_t dataBits, const canU8_t offset) {
/**--------------------------------------------- 法一 ---------------------------------------------------**/
//    canU64_t temp, mask0, temp1;
//    canU64_t* p0;
//    canU32_t* p1;
//    if (offset + dataBits <= 64) {
//        temp = (canU64_t)data & ((canU64_t)~(0XFFFFFFFFFFFFFFFF << dataBits)); // 要添加的数[00000000000000 10110]
//        temp = temp << offset;          // 要添加的数[00000000000000 10110 0000000000000]
//        p0 = (canU64_t*)Data; // 原始数据
//        // 制作掩膜[11111111111111 00000 1111111111111]
//        mask0 = (canU64_t)~(0XFFFFFFFFFFFFFFFF << offset) | (canU64_t)~(0XFFFFFFFFFFFFFFFF >> (64 - dataBits - offset));
//        temp1 = (*p0) & mask0;  // 得到[-------------- 00000 -------------]
//        (*p0) = temp | temp1;           //
//    }
//    if(offset + dataBits > 64 && offset + dataBits <= 96){
//
//        temp = (canU64_t)data & ((canU64_t)~(0XFFFFFFFFFFFFFFFF << dataBits));
//        p1 = (canU32_t*)Data;
//        p1 += 1;    // 使指针平移到32位之后的位置
//        temp = temp << (offset - 32);
//        mask0 = (canU64_t)~(0XFFFFFFFFFFFFFFFF << (offset - 32)) | (canU64_t)~(0XFFFFFFFFFFFFFFFF >> (96 - dataBits - offset));
//        p0 = (canU64_t*)p1;
//        temp1 = (*p0) & mask0;
//        (*p0) = temp | temp1;
//    }
//    else
//    {
//        temp = (canU64_t)data & ((canU64_t)~(0XFFFFFFFFFFFFFFFF << dataBits)); // 要添加的数[00000000000000 10110]
//        temp = temp << (offset - 64);          // 要添加的数[00000000000000 10110 0000000000000]
//        p0 = (canU64_t*)Data; // 原始数据
//        p0 = p0 + 1;
//        // 制作掩膜[11111111111111 00000 1111111111111]
//        mask0 = (canU64_t)~(0XFFFFFFFFFFFFFFFF << (offset - 64)) | (canU64_t)~(0XFFFFFFFFFFFFFFFF >> (128 - dataBits - offset));
//        temp1 = (*p0) & mask0;  // 得到[-------------- 00000 -------------]
//        (*p0) = temp | temp1;
//    }

/**--------------------------------------------- 法二 ---------------------------------------------------**/
    canU8_t id = offset / 8; // 数据应该在Data中的索引
    canU8_t bit_offset_front = offset % 8; // 数据在首字节中的偏移
    canU8_t bit_offset_tail = (offset + dataBits) % 8; //数据在尾部字节中的偏移
    canU8_t lenByte = (bit_offset_front + dataBits - 1) / 8 + 1;// 数据所占的字节数
    // cout << "head:" << (int)bit_offset_front << " tail:" << (int)bit_offset_tail << " lbty:"<<(int)lenByte <<endl;

    canU64_t temp = \
        ((canU64_t)data & (canU64_t)~(0XFFFFFFFF << dataBits)) << bit_offset_front;     // 将数据赋值
    //cout << "temp:"<<bitset<64>(temp)<<endl;
    canU8_t* p0 = (canU8_t*)&Data[id];  // 将指针指向开始操作的字节
    canU8_t* p1 = (canU8_t*)&temp;      // 指针指向缓冲区


    if (lenByte > 1)
    {
        canU8_t temp0_ = p1[0] | (canU8_t)~(0XFF << bit_offset_front);
        canU8_t p0_0_ = p0[0] | (canU8_t)(0XFF << bit_offset_front);
        p0[0] = p0_0_ & temp0_;
        canU8_t tempE_ = p1[lenByte - 1] | (canU8_t)(0XFF << bit_offset_tail);
        canU8_t p0_E_ = p0[lenByte - 1] | (canU8_t)~(0XFF << bit_offset_tail);

        p0[lenByte - 1] = p0_E_ & tempE_;
        if (bit_offset_tail == 0)p0[lenByte - 1] = p1[lenByte - 1];
        // 将缓冲区的数据赋值给原始数组
        for (int i = 1; i < lenByte - 1; i++)
        {
            p0[i] = p1[i];
        }
    }
    else
    {
        canU8_t temp0_ = (canU8_t)~(0XFF << bit_offset_front) | (canU8_t)(0XFF << bit_offset_tail);
        p0[0] = p1[0] |(p0[0] & temp0_);
    }
}

/* @:getBit,此函数只能获取小于64位长度的数据
 * 优点：速度块，一次操作64位.
 */
canU64_t getBit(const canU8_t* Data,const canU8_t dataBits, const canU8_t offset)
{
    canU64_t temp;
    canU64_t* p0;
    canU32_t* p1;

    if (offset + dataBits <= 64) {
        p0 = (canU64_t*)Data;
        temp = read_bit((*p0), (~(0XFFFFFFFFFFFFFFFF << dataBits)) << offset)>>offset;
    }
    else if (offset + dataBits > 64 && offset + dataBits <= 96) {
        p1 = (canU32_t*)Data;
        p1++;
        p0 = (canU64_t*)p1;
        temp = read_bit((*p0), (~(0XFFFFFFFFFFFFFFFF << dataBits)) << (offset - 32))>>(offset - 32);
    }
    else{
        p0 = (canU64_t*)Data;
        p0 = p0 + 1;
        temp = read_bit((*p0), (~(0XFFFFFFFFFFFFFFFF << dataBits)) << (offset - 64))>>(offset - 64);
    }

    return temp;
}

void getBit(const canU8_t* src,canU8_t* tar,canU8_t dataBits,canU8_t offset)
{   // TODO:操作位数大于64为的帧，使用移位的方法，速度慢一些。

}

